#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
#include "robo/Encoder.h"
#include "robo/Motor.h"
#include <string.h>

using namespace robo;

const float B = 0.5;   // distance between wheel
const float rL = 0.1;  // left wheel radius
const float rR = 0.1;  // right wheel radius
const float ticks_rev = 500;//500 ticks per revolution on the wheels

bool inited_odom = false;
long last_encL = 0;
long last_encR = 0;

double odomX = 0;
double odomY = 0;
double odomA = 0;

void recive_encoder(const Encoder::ConstPtr &msg)
{
   long dEncL=0, dEncR=0;
   if (inited_odom) {
     dEncL = msg->left - last_encL;
     dEncR = msg->right - last_encR;
   }
   double dAngL = dEncL / ticks_rev * 2*M_PI;
   double dAngR = dEncR / ticks_rev * 2*M_PI;

   double dd = (dAngR*rR + dAngL*rL) / 2;
   double da = (dAngR*rR - dAngL*rL) / B;

   odomX += dd*cos(odomA);
   odomY += dd*sin(odomA);
   odomA += da;
   
   last_encL = msg->left;
   last_encR = msg->right;
   inited_odom = true;
	printf("Odom X=%.3fm Y=%.3fm Theta=%.1fdeg\n",odomX, odomY, 180.0/M_PI*odomA);
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "EvaluateMotion");//Creates a node named "EvaluateMotion"
	ros::NodeHandle n;
   ros::Subscriber enc_sub = n.subscribe("/serial/encoder", 1000, recive_encoder);//when "/encoder" topic is revieved call recive_encoder function

	ros::Rate loop_rate(10000);

	while(ros::ok()){
     loop_rate.sleep();
     ros::spinOnce();
	}
	return 0;
}
